
import cv2
import numpy as np
import time

def random_partition(n,n_data):
    """return n random rows of data (and also the other len(data)-n rows)"""
    """返回数据的随机n行的索引,以及剩余数据的索引"""
    all_idxs = np.arange( n_data )
    np.random.shuffle(all_idxs)
    np.random.shuffle(all_idxs)
    idxs1 = all_idxs[:n]
    idxs2 = all_idxs[n:]
    return idxs1, idxs2

def getSubset(data1,data2,modelPoints,model):
    maxAttempts=1000
    NumOfData=len(data1)
    for att in range(maxAttempts):
        idxes=[]
        for i in range(modelPoints):
            while True:
                #idx=np.random.randint(0,NumOfData-1)
                idx=np.round(np.random.uniform(0,NumOfData-1))
                if idx in idxes:
                    continue
                idxes.append(idx)
                break
        #check
        if model.checkSamples(data1[idxes],data2[idxes],modelPoints):
            return idxes,True
    return idxes,False  #未采样到正确数据

def findInliers(data1,data2,modelParams,model,thresh):
    """计算给定模型下，内点数以及误差"""
    dist=model.compute_distance(data1,data2,modelParams)
    n=len(data1)
    goodCnt=np.sum(np.int8(dist[0]<thresh)*np.int8(dist[1]<thresh))
    goodIdxes=np.where(np.int8(dist[0]<thresh)*np.int8(dist[1]<thresh))
    if goodCnt>0:
        resErr=np.sum(dist[0]**2+dist[1]**2)/goodCnt
    else:
        resErr=np.inf
    return goodCnt,goodIdxes,resErr

def updataNumIters(confidence,inlierRatio,modelPoints,numIters):
    p=np.max([confidence,0.0])
    p=np.min([confidence,1.0])
    ep=inlierRatio
    num=np.log(1-p)
    denom=np.log(1-ep**modelPoints)

    return np.min([numIters,np.round(num/denom)])

def ransac(data1,data2,model,modelPoints,maxIter,thresh,confidence=0.99):
    """
    data - 数据点
    model - 待拟合的模型
    modelPoints - 拟合模型所需最小数据量
    maxIter - 算法最大迭代次数
    thresh - 一个数据点是否拟合模型的阀值
    d - 确定模型是否很好的拟合数据时所需的内点数量
    """
    maxGoodCnt=0
    bestGoodIdxes=[]
    numDatas=len(data1)
    bestModelParams=[]
    iter=0
    if modelPoints!=7:
        modelPoints=8
    while iter< maxIter:
        #随机取n个数据
        fit_idxes,found = getSubset(data1,data2,modelPoints,model)
        if not found:
            break
        iter=iter+1
        #调用七点算法,得到的modelParams为F矩阵
        modelParams=model.fit(data1[fit_idxes],data2[fit_idxes],modelPoints)
        #modelParams=model.fit(sd1,sd2,modelPoints)
        nmodels=len(modelParams)
        for i in range(nmodels):
            #计算在当前估计模型下的内点
            goodCnt,goodIdxes,err=findInliers(data1,data2,modelParams[i],model,thresh)
            if goodCnt >np.max([maxGoodCnt,modelPoints-1]):
                bestGoodIdxes=goodIdxes
                maxGoodCnt=goodCnt
                bestModelParams=modelParams[i]
                ##自适应地调整迭代次数
                maxIter=updataNumIters(confidence,goodCnt/numDatas,modelPoints,maxIter)
                print("maxIter=%d  inliers=%d"%(maxIter,maxGoodCnt))
    if maxGoodCnt >0 and iter>=maxIter:
        print("inliers=%d",maxGoodCnt)
        return bestModelParams,goodIdxes
    else:
        return None,None